//------------------------------------------------------------------------------
//  <copyright file="PathPlanningModelUpdater.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Motion
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using System.Windows;
    using System.Windows.Controls;
    using System.Windows.Media;
    using System.Windows.Media.Imaging;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Update path planning model for visualization.
    /// </summary>
    public class PathPlanningModelUpdater
    {
        /// <summary>
        /// The image we generate from the map and plan.
        /// </summary>
        private byte[] rawImage;

        /// <summary>
        /// Update visualization model.
        /// </summary>
        /// <param name="plan">Path plan.</param>
        /// <param name="mapCellsAhead">Number of map cells ahead to target.</param>
        /// <param name="callback">Callback given an image source.</param>
        /// <param name="control">Control with which to invoke the UI thread.</param>
        public void UpdateModel(PathPlan plan, int mapCellsAhead, Action<ImageSource> callback, UserControl control)
        {
            GlobalMap map = plan.Map;
            int width = map.Width;
            int height = map.Height;

            if (this.rawImage == null)
            {
                this.rawImage = new byte[width * height];
            }

            // global occupancy map visualized as black/white pixels
            for (int y = 0; y < width; y++)
            {
                for (int x = 0; x < height; x++)
                {
                    int occupancy = map.IsFree(x, y) ? 0 : 255;
                    this.rawImage[(width * height) - 1 - ((x * width) + y)] = (byte)occupancy;
                }
            }

            if (control != null)
            {
                control.Dispatcher.Invoke(() =>
                {
                    var visual = new DrawingVisual();
                    DrawingContext ctx = visual.RenderOpen();

                    var writeableBmp = new WriteableBitmap(width, height, 100, 100, PixelFormats.Gray8, null);
                    writeableBmp.WritePixels(new Int32Rect(0, 0, width, height), rawImage, width, 0);
                    ctx.DrawImage(writeableBmp, new Rect(0, 0, width, height));

                    Pose2D location = plan.CurrentPose;
                    Point2D<int> loc = PathPlanningCommon.PosePoint(location);
                    double locY = location.Y;
                    double locX = location.X;

                    // target location visualized as red circle
                    ctx.DrawEllipse(
                        Brushes.Red,
                        new Pen(Brushes.Black, 1),
                        this.MapCoordsToPoint(new Point2D<int>((int)plan.TargetPose.X, (int)plan.TargetPose.Y), width, height),
                        5,
                        5);

                    // current location visualized as blue circle
                    ctx.DrawEllipse(
                        Brushes.LightBlue,
                        new Pen(Brushes.Black, 1),
                        this.MapCoordsToPoint(loc, width, height),
                        4,
                        4);

                    if (plan != null)
                    {
                        IEnumerable<Point2D<int>> path = plan.Path;
                        int arrowLength = 50;

                        if (path != null)
                        {
                            // current heading visualized as blue arrow
                            double currentHeading = -plan.CurrentPose.Heading + MathConstants.PIOverTwo;
                            ctx.DrawLine(
                                new Pen(Brushes.LightBlue, 4) { EndLineCap = PenLineCap.Triangle },
                                this.MapCoordsToPoint(loc, width, height),
                                this.MapCoordsToPoint(
                                    new Point2D<int>(
                                        (int)(Math.Sin(currentHeading) * arrowLength + locX),
                                        (int)(Math.Cos(currentHeading) * arrowLength + locY)),
                                    width,
                                    height));

                            // target heading visualized as red arrow
                            Point2D<int> posePoint = PathPlanningCommon.PosePoint(location);
                            Point2D<int> targetPoint = PathPlanningCommon.TargetPoint(path, posePoint, mapCellsAhead);
                            double targetHeading = -PathPlanningCommon.TargetHeading(targetPoint, posePoint) + MathConstants.PIOverTwo;
                            ctx.DrawLine(
                                new Pen(Brushes.Red, 4) { EndLineCap = PenLineCap.Triangle },
                                this.MapCoordsToPoint(loc, width, height),
                                this.MapCoordsToPoint(
                                    new Point2D<int>(
                                        (int)(Math.Sin(targetHeading) * arrowLength + locX),
                                        (int)(Math.Cos(targetHeading) * arrowLength + locY)),
                                    width,
                                    height));

                            // path visualized in yellow
                            Pen yellowPen = new Pen(Brushes.Yellow, 1);
                            foreach (Point2D<int> p in path)
                            {
                                ctx.DrawEllipse(
                                    Brushes.Yellow,
                                    yellowPen,
                                    this.MapCoordsToPoint(p, width, height),
                                    1,
                                    1);
                            }
                        }
                    }

                    ctx.Close();
                    var bmp = new RenderTargetBitmap(width, height, 100, 100, new PixelFormat());
                    bmp.Render(visual);
                    callback(bmp);
                });
            }
        }

        /// <summary>
        /// Convert map coordinates to pixel coordinates.
        /// </summary>
        /// <param name="point">Point on the map.</param>
        /// <param name="width">Map width.</param>
        /// <param name="height">Map height.</param>
        /// <returns>Pixel point.</returns>
        private Point MapCoordsToPoint(Point2D<int> point, double width, double height)
        {
            return new Point(width - point.Y, height - point.X);
        }
    }
}
